import copy
from Avoidance import pipe_preprocess
from Methods.block import drawing_add_blocks
from Method import collision_detection, get_cos_value, pipe_dian2, blocks_select, rotation_2D, get_Unit_vector
from Source.CWJ import qyhf



# list减法
def list_subtraction(*args):
    result = list(map(lambda x, y: x - y, args[1], args[0]))
    return result


# 输入面的4个坐标，输出1个法向量
# def find_normal_vector(*args):
#     if args[4] == 'left':
#         surface_vector1 = list_subtraction(args[0], args[1])  # map映射对应数值关系；lambda表达式
#         surface_vector2 = list_subtraction(args[0], args[3])
#     else:
#         surface_vector1 = list_subtraction(args[0], args[3])  # map映射对应数值关系；lambda表达式
#         surface_vector2 = list_subtraction(args[0], args[1])
#     normal_vector_i = surface_vector1[1] * surface_vector2[2] - surface_vector1[2] * surface_vector2[1]
#     normal_vector_j = surface_vector1[2] * surface_vector2[0] - surface_vector1[0] * surface_vector2[2]
#     normal_vector_k = surface_vector1[0] * surface_vector2[1] - surface_vector1[1] * surface_vector2[0]
#     sum = (normal_vector_i ** 2 + normal_vector_j ** 2 + normal_vector_k ** 2) ** 0.5
#     return [normal_vector_i / sum, normal_vector_j / sum, normal_vector_k / sum]


def pipe_LorRmove(pipe, distance, direction):  # 管道左右移动
    # 管道坐标输入
    point_8 = pipe_dian2([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle])  # 输入管道的s e w h
    left_vector, right_vector = [], []
    if direction == 'left':
        vector = []
        for i in range(3):
            vector.append(pipe.end[i] - pipe.start[i])
        left_vector = rotation_2D(get_Unit_vector(vector), 90)
        # left_vector = find_normal_vector(*point_8[-4:], 'left')  # point_8[:4]左面的四个点坐标
        for i in range(3):
            pipe.start[i] += distance * left_vector[i]
            pipe.end[i] += distance * left_vector[i]
        pass
    if direction == 'right':
        vector = []
        for i in range(3):
            vector.append(pipe.end[i] - pipe.start[i])
        right_vector = rotation_2D(get_Unit_vector(vector), -90)
        for i in range(3):
            pipe.start[i] += distance * right_vector[i]
            pipe.end[i] += distance * right_vector[i]
        pass
    return left_vector, right_vector

def pipe_expand(pipe, distance, direction):  # 管道向左右扩充一定距离
    new_pipe = copy.deepcopy(pipe)
    new_pipe[2] = new_pipe[2] + distance
    if direction == 'left':
        vector = []
        for i in range(3):
            vector.append(pipe[1][i] - pipe[0][i])
        left_vector = rotation_2D(get_Unit_vector(vector), 90)
        for i in range(3):
            new_pipe[0][i] = pipe[0][i] + (new_pipe[2] / 2 - pipe[2] / 2) * left_vector[i]
            new_pipe[1][i] = pipe[1][i] + (new_pipe[2] / 2 - pipe[2] / 2) * left_vector[i]
    if direction == 'right':
        vector = []
        for i in range(3):
            vector.append(pipe[1][i] - pipe[0][i])
        right_vector = rotation_2D(get_Unit_vector(vector), -90)
        for i in range(3):
            new_pipe[0][i] = pipe[0][i] + (new_pipe[2] / 2 - pipe[2] / 2) * right_vector[i]
            new_pipe[1][i] = pipe[1][i] + (new_pipe[2] / 2 - pipe[2] / 2) * right_vector[i]
    return new_pipe


# new_pipe = pipe_expand([[0, 0, 10], [0, 10, 10], 2, 2, 0], 5, 'right')
# print('扩充', new_pipe)


# insulation是保温层大小，installation是安装空间大小
def pipe_move_crash(pipe, block_arr, insulation=0, installation=0, adjust_distance=200):
    # 检测左边200mm内和右边200mm内是否有碰撞
    blockList = blocks_select([[pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle]], block_arr, 5000)  # 获取管道绑定的障碍物
    # print('原来', [pipe.start, pipe.end, pipe.size.width, pipe.size.height])
    left_newpipe = pipe_expand([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle], adjust_distance, 'left') # 向左移动的距离要是保温层+安装空间
    # print("左expand后", left_newpipe)
    left_flag, right_flag = 0, 0
    # left_newpipe_expand = left_newpipe[:]
    # left_newpipe_expand[2] = left_newpipe[2] + insulation + installation
    # left_newpipe_expand[3] = left_newpipe[3] + insulation + installation  # 管道宽高分别加保温层和安装空间
    for i in range(len(blockList)):
        is_collision = collision_detection(left_newpipe, blockList[i])
        if is_collision:
            # type = qyhf.check([blockList[i]], blockBase)
            left_flag = 1
            # print('管道左扩充后碰到障碍物')
            break
    right_newpipe = pipe_expand([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle], adjust_distance, 'right')
    # print("右expand后", right_newpipe)
    # right_newpipe_expand = right_newpipe[:]
    # right_newpipe_expand[2] = right_newpipe[2] + insulation + installation
    # right_newpipe_expand[3] = right_newpipe[3] + insulation + installation
    for i in range(len(blockList)):
        is_collision = collision_detection(right_newpipe, blockList[i])
        if is_collision:
            # type = qyhf.check([blockList[i]], blockBase)
            right_flag = 1
            # print('管道右扩充后碰到障碍物')
            break
    # print('left_flag right_flag', left_flag, right_flag)
    if not left_flag and not right_flag:  # 两边都没有碰撞
        return -1
    elif left_flag and right_flag:  # 两边都有碰撞
        return -2
    else:  # 一边碰撞，一边不碰撞
        if left_flag == 1:
            return 0
        else:
            return 1

    # 判断左移200是否碰撞

    # 判断右移200是否碰撞
    # 如果左边200和右边200内都有碰撞，返回原管道
    # 如果一边有碰撞，一边没有，就向没有碰撞的地方移动，移动完后另外一段和这段管道连接的也要移动


def pipes_adjust_main(pipeBase, block_arr):
    pipeBase._pipeGroups = sorted(pipeBase._pipeGroups, key=lambda x : x.pipeGroupNo)
    for i, pipeGroup in enumerate(pipeBase._pipeGroups):
        # if pipeGroup.pipeGroupNo == pipeGroupNo:
        is_single_pipelist = False
        if len(pipeGroup._pipeLists) == 1:
            is_single_pipelist = True
        for pipeList in pipeGroup._pipeLists:
            try:
                new_pipeList = pipes_adjust(pipeList, block_arr, is_single_pipelist, pipeBase._pipeGroups[i-1], pipeGroup.type)
            except:
                print('微调错误')
                block_arr.extend(pipeList)
                continue
            block_arr.extend(new_pipeList)
    return

def pipes_adjust(pipeList, block_arr, is_single_piplist, pipeGroupPrev, pi_type):  # 管道列的移动
    for i, pipe in enumerate(pipeList._pipes):
        if is_single_piplist and len(pipeGroupPrev._pipeLists) == 1:
            pipePrev = pipeGroupPrev._pipeLists[0]._pipes[0]
            cos_value = get_cos_value([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle], 
            [pipePrev.start, pipePrev.end, pipePrev.size.width, pipePrev.size.height, pipePrev.angle])
            if 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
                information = pipe_preprocess([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle], 
                [pipePrev.start, pipePrev.end, pipePrev.size.width, pipePrev.size.height, pipePrev.angle], cos_value, pi_type)
                pipe.start = information[0][0][0][0]
                pipe.end = information[0][0][0][1]
                pipe.size.width = information[0][0][0][2]
                pipe.size.height = information[0][0][0][3]
        if is_single_piplist:
            crash_flag = pipe_move_crash(pipe, block_arr)
            while crash_flag >= 0:
                if crash_flag == 0:
                    # 左边碰撞
                    _, right_vector = pipe_LorRmove(pipe, 200, 'right')
                    for k in range(len(pipeList._pipes)):
                        if k != i:
                            for j in range(3):
                                pipeList._pipes[k].start[j] += 200 * right_vector[j]
                                pipeList._pipes[k].end[j] += 200 * right_vector[j]
                elif crash_flag == 1:
                    # 右边碰撞
                    left_vector, _ = pipe_LorRmove(pipe, 200, 'left')
                    for k in range(len(pipeList._pipes)):
                        if k != i:
                            for j in range(3):
                                pipeList._pipes[k].start[j] += 200 * left_vector[j]
                                pipeList._pipes[k].end[j] += 200 * left_vector[j]
                for i, pipe in enumerate(pipeList._pipes):
                    if pipe_move_crash(pipe, block_arr) >= 0:
                        flag = 1
                        break
                    else:
                        flag = 0
                if flag == 1:
                    continue
                break
        else:
            if i != 0 and i != len(pipeList._pipes) - 1:  # 中间的管道段
                    crash_flag = pipe_move_crash(pipe, block_arr)
                    if crash_flag == 0:
                        # 左边碰撞
                        _, right_vector = pipe_LorRmove(pipe, 200, 'right')
                        for j in range(3):
                            pipeList._pipes[i + 1].start[j] += 200 * right_vector[j]
                            pipeList._pipes[i - 1].end[j] += 200 * right_vector[j]
                    elif crash_flag == 1:
                        # 右边碰撞
                        left_vector, _ = pipe_LorRmove(pipe, 200, 'left')
                        for j in range(3):
                            pipeList._pipes[i + 1].start[j] += 200 * left_vector[j]
                            pipeList._pipes[i - 1].end[j] += 200 * left_vector[j]
    pipe_list = []
    for pipe in pipeList._pipes:
        pipe_list.append([pipe.start, pipe.end, pipe.size.width, pipe.size.height])
    return pipe_list

def draw_figure(pipeGroups):
    pipe_coordinates = []
    # 获取三维坐标中的最大和最小坐标，作为坐标系的参数
    min_x = min_y = min_z = float('inf')
    max_x = max_y = max_z = 0
    for pipeGroup in pipeGroups:
        for pipelist in pipeGroup:
            for i in range(len(pipelist)):
                if min_x > min(pipelist[i][0][0], pipelist[i][1][0]):
                    min_x = min(pipelist[i][0][0], pipelist[i][1][0])
                if max_x < max(pipelist[i][0][0], pipelist[i][1][0]):
                    max_x = max(pipelist[i][0][0], pipelist[i][1][0])
                if min_y > min(pipelist[i][0][1], pipelist[i][1][1]):
                    min_y = min(pipelist[i][0][1], pipelist[i][1][1])
                if max_y < max(pipelist[i][0][1], pipelist[i][1][1]):
                    max_y = max(pipelist[i][0][1], pipelist[i][1][1])
                if min_z > min(pipelist[i][0][2], pipelist[i][1][2]):
                    min_z = min(pipelist[i][0][2], pipelist[i][1][2])
                if max_z < max(pipelist[i][0][2], pipelist[i][1][2]):
                    max_z = max(pipelist[i][0][2], pipelist[i][1][2])
                pipe_coordinates.append(qyhf.dian2(pipelist[i]))
    drawing_add_blocks(pipe_coordinates, min_x, max_x, min_y, max_y, min_z, max_z)



if __name__ == '__main__':
    # pipe = [[500, 0, 500], [500, 500, 500], 200, 200, 0]
    # # pipe, _, right_vector = pipe_LorRmove(pipe, 200, 'right')
    # block_arr = [[[900, 0, 500], [900, 500, 500], 400, 400]]
    # pipe, _, right_vector = pipe_LorRmove(pipe, 200, 'right')
    # pipe_dian2
    # # print(collision_detection([[500, 0, 500], [500, 500, 500], 600, 200, 0], [[900, 0, 500], [900, 500, 500], 400, 400]))
    # f = pipe_move_crash(pipe, block_arr, insulation=0, installation=0, adjust_distance=200)
    dian = pipe_dian2([[0, 100, 100], [200, 100, 100], 200, 200, 0])
    print(dian)
    dian2 = pipe_dian2([[100, 0, 100], [100, 200, 100], 200, 200, 0])
    print(dian2)
